package Physics;

public class Contact {
	////////////////////////////
	// private member variables
	//////////////////////////
	/**
	 * Transform matrix that converts contact
	 * coordinates into world coordinates
	 */
	Matrix3 m_contactToWorld = new Matrix3();
	
	/** 
	 * Closing velocity at the point of contact
	 */
	Vector3 m_contactVelocity = new Vector3();
	
	/**
	 * The required change in velocity to
	 * resolve the contact.
	 */
	float m_desiredDeltaVelocity;
	
	/**
	 * World space positions of the contact point
	 * relative to each body's Center of Mass (COM)
	 */
	Vector3 m_relativeContactPosition[] = new Vector3[2] ;
	
	//////////////////////////
	// Public data
	//////////////////////////
	/**
	 * Holds the objects involved in the contact.
	 * The second should be null in the case where
	 * we are not colliding with something that
	 * can be moved (e.g. between a box and the floor)
	 */
	public RigidBody m_objects[] = new RigidBody[2];
	
	/**
	 * The coefficient of friction between the two
	 * objects.
	 */
	public float m_friction;
	
	/**
	 * The coefficient of restitution between the two
	 * objects.
	 */
	public float m_restitution;
	
	/**
	 * The contact point in world coordinates
	 */
	public Vector3 m_contactPoint = new Vector3();
	
	/**
	 * The contact normal in world-coordinates
	 */
	public Vector3 m_contactNormal = new Vector3();
	
	/**
	 * The penetration depth
	 */
	public float m_penetration;
		
	//////////////////////////
	// Private member functions
	//////////////////////////
	/**
	 * Calculate internal data for the next
	 * iteration.
	 */
	void calculateInternals(float dt)
	{
		// Check if the first object is NULL, and swap if it is.
	    if (m_objects[0] == null) 
			swapBodies();
	    
		assert(m_objects[0] == null);

	    // Calculate an set of axis at the contact point.
	    calculateContactBasis();

	    // Store the relative position of the contact relative to each body
	    m_relativeContactPosition[0] = m_contactPoint.subtract(m_objects[0].getPosition());
	    if (m_objects[1] != null) {
	        m_relativeContactPosition[1] = m_contactPoint.subtract(m_objects[1].getPosition());
	    }

	    // Find the relative velocity of the bodies at the contact point.
	    m_contactVelocity = calculateLocalVelocity(0, dt);
	    if (m_objects[1] != null) {
	        m_contactVelocity.subtractAndSet(calculateLocalVelocity(1, dt));
	    }

	    // Calculate the desired change in velocity for resolution
	    calculateDesiredDeltaVelocity(dt);

		// apply code here that will damage the objects.
		// damage should be proportional to F = m*a
		// which is F = m*dv/dt
		//real dvdt = contactVelocity.magnitude() / duration;
		float dvdt = m_desiredDeltaVelocity;
		//real force0 = dvdt / (sqrt(objects[0]->getMass()));
		//real force0 = dvdt / objects[0]->getMass();
		float force0 = dvdt;
		
		float force1 = 0;
		if (m_objects[1] != null)
			//force1 = dvdt / (sqrt(objects[1]->getMass()));
			//force1 = dvdt / objects[1]->getMass();
			force1 = dvdt;

		// add this collision to the possible game collisions
		// if the force isn't strong enough, it won't bother adding
		// it to the list
		
		/**
		 * TODO: Implement the health manager class
		 */
		//HEALTHMANAGER->AddCollision(m_objects[0], force0);
		//if (m_objects[1] != null)
			//HEALTHMANAGER->AddCollision(m_objects[1], force1);
			
	}
	
	/**
	 * Reverses the contact. Swaps the two bodies
	 * and the contact normal.
	 */
	void swapBodies()
	{
		m_contactNormal.multiplyAndSet(-1);

		RigidBody temp = m_objects[0];
		m_objects[0] = m_objects[1];
		m_objects[1] = temp;
	}
	
	/**
	 * Updates the awake state of both bodies
	 * to match one-another.
	 */
	void matchAwakeState()
	{
		// don't need to be awake for collisions with the world
		if (m_objects[1] == null)
			return;

		boolean body0awake = m_objects[0].getAwake();
		boolean body1awake = m_objects[1].getAwake();

		
		if (body0awake == true || body1awake == true)
		{
			// wake up both objects
			m_objects[0].setAwake(true);
			m_objects[1].setAwake(true);
		}
		
	}
	
	/**
	 * calculates the desired velocity to resolve
	 * this contact.
	 */
	void calculateDesiredDeltaVelocity(float dt)
	{
		float velocityLimit = 0.25f;

	    // Calculate the acceleration induced velocity accumulated this frame
	    float velocityFromAcc = 0;

	    if (m_objects[0].getAwake())
	    {
	    	/**
	    	 * WARNING: slight change here from the original code. velocityFromAcc
	    	 * was never set here, so that line may need to be commented out if
	    	 * things go wrong. This seems to have been an oversight in it's original
	    	 * creation, however. The function was run without actually doing anything.
	    	 * Might explain some of the engine's weird collision behaviour? 
	    	 */
	    	Vector3 temp = m_objects[0].getLastFrameAcceleration().multiply(dt);
	    	velocityFromAcc = temp.dotProduct(m_contactNormal);
	    	 
	    }

	    if (m_objects[1] != null && m_objects[1].getAwake())
	    {
	    	Vector3 temp = m_objects[1].getLastFrameAcceleration().multiply(dt);
	    	velocityFromAcc -= (temp.dotProduct(m_contactNormal));
	    }

	    // If the velocity is very slow, limit the restitution
	    float thisRestitution = m_restitution;
	    if (Math.abs(m_contactVelocity.x) < velocityLimit)
	    {
	        thisRestitution = 0.0f;
	    }

	    // Combine the bounce velocity with the removed
	    // acceleration velocity (assumed to be the velocity
		// from gravity)
	    m_desiredDeltaVelocity = -m_contactVelocity.x - thisRestitution * (m_contactVelocity.x - velocityFromAcc);
	}
	
	/**
	 * Calculates the velocity of the contact point
	 * on the body.
	 */
	
	Vector3 calculateLocalVelocity(int bodyIndex, float dt)
	{
		RigidBody thisBody = m_objects[bodyIndex];

	    // Work out the velocity of the contact point.
	    Vector3 velocity = (thisBody.getRotation()).crossProduct(m_relativeContactPosition[bodyIndex]);
	    velocity.addAndSet(thisBody.getVelocity());

	    // Turn the velocity into contact-coordinates.
	    Vector3 contactVelocity = m_contactToWorld.transformTranspose(velocity);

	    // Calculate the amount of velocity that is due to forces without
	    // reactions.
	    Vector3 accVelocity = (thisBody.getLastFrameAcceleration()).multiply(dt);

	    // Calculate the velocity in contact-coordinates.
	    accVelocity = m_contactToWorld.transformTranspose(accVelocity);

	    // We ignore any component of acceleration in the contact normal
	    // direction, we are only interested in planar acceleration
	    accVelocity.x = 0;

	    // Add the planar velocities - if there's enough friction they will
	    // be removed during velocity resolution
	    contactVelocity.addAndSet(accVelocity);

	    // And return it
	    return contactVelocity;
	}
	
	/**
	 * calculates the orthonormal basis of the contact point
	 */
	void calculateContactBasis()
	{
		Vector3 [] contactTangent = new Vector3[2];
		contactTangent[0] = new Vector3(0.0f, 0.0f, 0.0f);
		contactTangent[1] = new Vector3(0.0f, 0.0f, 0.0f);

	    // Check whether the Z-axis is nearer to the X or Y axis
	    if (Math.abs(m_contactNormal.x) > Math.abs(m_contactNormal.y))
	    {
	        // Scaling factor to ensure the results are normalised
	        float s = 1.0f/(float)Math.sqrt(m_contactNormal.z*m_contactNormal.z +
	        		m_contactNormal.x*m_contactNormal.x);

	        // The new X-axis is at right angles to the world Y-axis
	        contactTangent[0].x = m_contactNormal.z*s;
	        contactTangent[0].y = 0;
	        contactTangent[0].z = -m_contactNormal.x*s;

	        // The new Y-axis is at right angles to the new X- and Z- axes
	        contactTangent[1].x = m_contactNormal.y*contactTangent[0].x;
	        contactTangent[1].y = m_contactNormal.z*contactTangent[0].x -
	        		m_contactNormal.x*contactTangent[0].z;
	        contactTangent[1].z = -m_contactNormal.y*contactTangent[0].x;
	    }
	    else
	    {
	        // Scaling factor to ensure the results are normalised
	        float s = 1.0f/(float)Math.sqrt(m_contactNormal.z*m_contactNormal.z +
	        		m_contactNormal.y*m_contactNormal.y);
	        


	        // The new X-axis is at right angles to the world X-axis
	        contactTangent[0].x = 0;
	        contactTangent[0].y = -m_contactNormal.z*s;
	        contactTangent[0].z = m_contactNormal.y*s;

	        // The new Y-axis is at right angles to the new X- and Z- axes
	        contactTangent[1].x = m_contactNormal.y*contactTangent[0].z -
	        		m_contactNormal.z*contactTangent[0].y;
	        contactTangent[1].y = -m_contactNormal.x*contactTangent[0].z;
	        contactTangent[1].z = m_contactNormal.x*contactTangent[0].y;
	    }

	    // Make a matrix from the three vectors.
	    m_contactToWorld.setComponents(
	    		m_contactNormal,
	        contactTangent[0],
	        contactTangent[1]);
	}
	
	/**
	 * Applies an impulse to the given body
	 * and returns the change in velocity
	 * NOTE: Never implemented in original code...
	 */
	//void applyImpulse(Vector3 impulse, RigidBody body, Vector3 velocityChange, Vector3 rotationChange)
	//{
	//}	
	
	/**
	 * Applies and impulse that is weighted by the
	 * object's inertia
	 */
	void applyVelocityChange(Vector3 velocityChange[], Vector3 rotationChange[])
	{
		// Get hold of the inverse mass and inverse inertia tensor, both in
	    // world coordinates.
	    Matrix3 [] inverseInertiaTensor = new Matrix3[2];
	    
	    for(int i = 0; i < 2; ++i)
	    	inverseInertiaTensor[i] = new Matrix3();
	    
	    inverseInertiaTensor[0] = m_objects[0].getInverseInertiaTensorWorld();
	    if (m_objects[1] != null)
	    	inverseInertiaTensor[1] = m_objects[1].getInverseInertiaTensorWorld();

	    // We will calculate the impulse for each contact axis
	    Vector3 impulseContact;

	    if (m_friction == 0.0f)
	    {
	        // Use the short format for frictionless contacts
	        impulseContact = calculateFrictionlessImpulse(inverseInertiaTensor);
	    }
	    else
	    {
	        // Otherwise we may have impulses that aren't in the direction of the
	        // contact, so we need the more complex version.
	        impulseContact = calculateFrictionImpulse(inverseInertiaTensor);
	    }

	    // Convert impulse to world coordinates
	    Vector3 impulse = m_contactToWorld.transform(impulseContact);

	    // Split in the impulse into linear and rotational components
	    Vector3 impulsiveTorque = m_relativeContactPosition[0].crossProduct(impulse);
	    rotationChange[0] = inverseInertiaTensor[0].transform(impulsiveTorque);
	    velocityChange[0].clear();
	    velocityChange[0].addScaledVector(impulse, m_objects[0].getInverseMass());

	    // Apply the changes
		if (m_objects[0].hasFiniteMass())
		{
			m_objects[0].addVelocity(velocityChange[0]);
			m_objects[0].addRotation(rotationChange[0]);
		}
		
		if (m_objects[1] != null)
	    {
	        // Work out body two's linear and angular changes
	        impulsiveTorque = impulse.crossProduct(m_relativeContactPosition[1]);
	        rotationChange[1] = inverseInertiaTensor[1].transform(impulsiveTorque);
	        velocityChange[1].clear();
	        velocityChange[1].addScaledVector(impulse, -m_objects[1].getInverseMass());

	        // And apply them.

			if (m_objects[1].hasFiniteMass())
			{
				m_objects[1].addVelocity(velocityChange[1]);
				m_objects[1].addRotation(rotationChange[1]);
			}
	    }
	}
	
	/**
	 * Performs a penetration resolution, weighted
	 * by the object's inertia
	 */
	void applyPositionChange(Vector3 linearChange[], Vector3 angularChange[], float penetration)
	{
		float angularLimit = 0.2f;
	    float angularMove[] = new float[2];
	    float linearMove[] = new float[2];

	    float totalInertia = 0;
	    float linearInertia[] = new float[2];
	    float angularInertia[] = new float[2];

	    // We need to work out the inertia of each object in the direction
	    // of the contact normal, due to angular inertia only.
	    for (int i = 0; i < 2; i++) if (m_objects[i] != null)
	    {
			//if (!objects[i]->hasFiniteMass())
				//continue;

	        Matrix3 inverseInertiaTensor;
	        inverseInertiaTensor = m_objects[i].getInverseInertiaTensorWorld();

	        // Use the same procedure as for calculating frictionless
	        // velocity change to work out the angular inertia.
	        Vector3 angularInertiaWorld = m_relativeContactPosition[i].crossProduct(m_contactNormal);
	        angularInertiaWorld = inverseInertiaTensor.transform(angularInertiaWorld);
	        angularInertiaWorld = angularInertiaWorld.crossProduct(m_relativeContactPosition[i]);
	        angularInertia[i] = angularInertiaWorld.dotProduct(m_contactNormal);

	        // The linear component is simply the inverse mass
	        linearInertia[i] = m_objects[i].getInverseMass();

	        // Keep track of the total inertia from all components
	        totalInertia += linearInertia[i] + angularInertia[i];

	        // We break the loop here so that the totalInertia value is
	        // completely calculated (by both iterations) before
	        // continuing.
	    }

	    // Loop through again calculating and applying the changes
	    for (int i = 0; i < 2; i++) if (m_objects[i] != null)
	    {

	        // The linear and angular movements required are in proportion to
	        // the two inverse inertias.
	        float sign = (i == 0)?1:-1;
	        angularMove[i] =
	            sign * penetration * (angularInertia[i] / totalInertia);
	        linearMove[i] =
	            sign * penetration * (linearInertia[i] / totalInertia);

	        // To avoid angular projections that are too great (when mass is large
	        // but inertia tensor is small) limit the angular move.
	        Vector3 projection = new Vector3(m_relativeContactPosition[i]);
	        projection.addScaledVector(
	            m_contactNormal,
	            -m_relativeContactPosition[i].dotProduct(m_contactNormal)
	            );

	        // Use the small angle approximation for the sine of the angle (i.e.
	        // the magnitude would be sine(angularLimit) * projection.magnitude
	        // but we approximate sine(angularLimit) to angularLimit).
	        float maxMagnitude = angularLimit * projection.magnitude();

	        if (angularMove[i] < -maxMagnitude)
	        {
	            float totalMove = angularMove[i] + linearMove[i];
	            angularMove[i] = -maxMagnitude;
	            linearMove[i] = totalMove - angularMove[i];
	        }
	        else if (angularMove[i] > maxMagnitude)
	        {
	            float totalMove = angularMove[i] + linearMove[i];
	            angularMove[i] = maxMagnitude;
	            linearMove[i] = totalMove - angularMove[i];
	        }

	        // We have the linear amount of movement required by turning
	        // the rigid body (in angularMove[i]). We now need to
	        // calculate the desired rotation to achieve that.
	        if (angularMove[i] == 0)
	        {
	            // Easy case - no angular movement means no rotation.
	            angularChange[i].clear();
	        }
	        else
	        {
	            // Work out the direction we'd like to rotate in.
	            Vector3 targetAngularDirection =
	                m_relativeContactPosition[i].crossProduct(m_contactNormal);

	            Matrix3 inverseInertiaTensor;
	            inverseInertiaTensor = m_objects[i].getInverseInertiaTensorWorld();

	            // Work out the direction we'd need to rotate to achieve that
	            angularChange[i] =
	                (inverseInertiaTensor.transform(targetAngularDirection)).multiply((angularMove[i] / angularInertia[i]));
	        }

	        // Velocity change is easier - it is just the linear movement
	        // along the contact normal.
	        Vector3 vec = new Vector3(m_contactNormal);
	        linearChange[i] = vec.multiply(linearMove[i]);

	        // Now we can start to apply the values we've calculated.
	        // Apply the linear movement
	        Vector3 pos = m_objects[i].getPosition();
	        pos.addScaledVector(m_contactNormal, linearMove[i]);
			if (m_objects[i].hasFiniteMass())
				m_objects[i].setPosition(pos);

	        // And the change in orientation
	        Quaternion q = m_objects[i].getOrientation();
	        q.addScaledVector(angularChange[i], (1.0f));
	        if (m_objects[i].hasFiniteMass())
				m_objects[i].setOrientation(q);

	        // We need to calculate the derived data for any body that is
	        // asleep, so that the changes are reflected in the object's
	        // data. Otherwise the resolution will not change the position
	        // of the object, and the next collision detection round will
	        // have the same penetration.
	        if (!m_objects[i].getAwake()) 
	        	m_objects[i].calculateDerivedData();
	    }
	}
	
	/**
	 * Calculates the impulse needed for contact resolution
	 * assuming zero friction
	 */
	Vector3 calculateFrictionlessImpulse(Matrix3 inverseInertiaTensor[])
	{
		Vector3 impulseContact = new Vector3();

	    // Build a vector that shows the change in velocity in
	    // world space for a unit impulse in the direction of the contact
	    // normal.
	    Vector3 deltaVelWorld = m_relativeContactPosition[0].crossProduct(m_contactNormal);
	    deltaVelWorld = inverseInertiaTensor[0].transform(deltaVelWorld);
	    deltaVelWorld = deltaVelWorld.crossProduct(m_relativeContactPosition[0]);

	    // Work out the change in velocity in contact coordiantes.
	    float deltaVelocity = deltaVelWorld.dotProduct(m_contactNormal);

	    // Add the linear component of velocity change
	    deltaVelocity += m_objects[0].getInverseMass();

	    // Check if we need to the second body's data
	    if (m_objects[1] != null)
	    {
	        // Go through the same transformation sequence again
	        deltaVelWorld = m_relativeContactPosition[1].crossProduct(m_contactNormal);
	        deltaVelWorld = inverseInertiaTensor[1].transform(deltaVelWorld);
	        deltaVelWorld = deltaVelWorld.crossProduct(m_relativeContactPosition[1]);

	        // Add the change in velocity due to rotation
	        deltaVelocity += deltaVelWorld.dotProduct(m_contactNormal);

	        // Add the change in velocity due to linear motion
	        deltaVelocity += m_objects[1].getInverseMass();
	    }

	    // Calculate the required size of the impulse
	    impulseContact.x = m_desiredDeltaVelocity / deltaVelocity;
	    impulseContact.y = 0;
	    impulseContact.z = 0;
	    return impulseContact;
	}
	
	/**
	 * Calculates the impulse needed for contact resolution
	 * assuming non-zero friction
	 */
	Vector3 calculateFrictionImpulse(Matrix3 inverseInertiaTensor[])
	{
		Vector3 impulseContact;
	    float inverseMass = m_objects[0].getInverseMass();

	    // The equivalent of a cross product in matrices is multiplication
	    // by a skew symmetric matrix - we build the matrix for converting
	    // between linear and angular quantities.
	    Matrix3 impulseToTorque = new Matrix3();;
	    impulseToTorque.setSkewSymmetric(m_relativeContactPosition[0]);

	    // Build the matrix to convert contact impulse to change in velocity
	    // in world coordinates.
	    Matrix3 deltaVelWorld = new Matrix3(impulseToTorque);
	    deltaVelWorld.multiplyAndSet(inverseInertiaTensor[0]);
	    deltaVelWorld.multiplyAndSet(impulseToTorque);
	    deltaVelWorld.multiplyAndSet(-1.0f);

	    // Check if we need to add body two's data
	    if (m_objects[1] != null)
	    {
	        // Set the cross product matrix
	        impulseToTorque.setSkewSymmetric(m_relativeContactPosition[1]);

	        // Calculate the velocity change matrix
	        Matrix3 deltaVelWorld2 = new Matrix3(impulseToTorque);
	        deltaVelWorld2.multiplyAndSet(inverseInertiaTensor[1]);
	        deltaVelWorld2.multiplyAndSet(impulseToTorque);
	        deltaVelWorld2.multiplyAndSet(-1.0f);

	        // Add to the total delta velocity.
	        deltaVelWorld.addAndSet(deltaVelWorld2);

	        // Add to the inverse mass
	        inverseMass += m_objects[1].getInverseMass();
	    }

	    // Do a change of basis to convert into contact coordinates.
	    Matrix3 mat = new Matrix3(m_contactToWorld);
	    Matrix3 deltaVelocity = mat.transpose();
	    deltaVelocity.multiplyAndSet(deltaVelWorld);
	    deltaVelocity.multiplyAndSet(m_contactToWorld);

	    // Add in the linear velocity change
	    deltaVelocity.data[0] += inverseMass;
	    deltaVelocity.data[4] += inverseMass;
	    deltaVelocity.data[8] += inverseMass;

	    // Invert to get the impulse needed per unit velocity
	    Matrix3 impulseMatrix = deltaVelocity.inverse();

	    // Find the target velocities to kill
	    Vector3 velKill = new Vector3(m_desiredDeltaVelocity,
	        -m_contactVelocity.y,
	        -m_contactVelocity.z);

	    // Find the impulse to kill target velocities
	    impulseContact = impulseMatrix.transform(velKill);

	    // Check for exceeding friction
	    float planarImpulse = (float)Math.sqrt(
	        impulseContact.y*impulseContact.y +
	        impulseContact.z*impulseContact.z
	        );
	    if (planarImpulse > impulseContact.x * m_friction)
	    {
	        // We need to use dynamic friction
	        impulseContact.y /= planarImpulse;
	        impulseContact.z /= planarImpulse;

	        impulseContact.x = deltaVelocity.data[0] +
	            deltaVelocity.data[1]*m_friction*impulseContact.y +
	            deltaVelocity.data[2]*m_friction*impulseContact.z;
	        impulseContact.x = m_desiredDeltaVelocity / impulseContact.x;
	        impulseContact.y *= m_friction * impulseContact.x;
	        impulseContact.z *= m_friction * impulseContact.x;
	    }
	    return impulseContact;
	}
	
	//////////////////////////
	// public member functions
	//////////////////////////
	/**
	 * Sets the public data of this contact.
	 */
	public void setBodyData(RigidBody one, RigidBody two, float friction, float restitution)
	{
		m_objects[0] = one;
		m_objects[1] = two;
		m_friction = friction;
		m_restitution = restitution;
	}
}
